﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace HostComputer
{
    internal class ClientContext
    {
        const int frameByteLength= 18;
        byte[] frameData = new byte[frameByteLength];
        int status = 0;
        int count = 0;

        internal void process(byte[] buf)
        {
            foreach (var item in buf)
            {
                switch(status) {
                    case 1: { 
                            if(item == 0x98)
                            {
                                status = 2;
                            }
                            break;
                        }
                    case 2: {
                            frameData[count] = item;
                            count++;
                            if(count == frameData.Length)
                            {
                                count = 0;
                                parseContent(frameData);
                                frameData = new byte[frameByteLength];
                                status = 1;
                            }
                            break;
                        }
                    default:
                        {
                            status = 1;
                            break;
                        }
                
                
                }
            }
        }

        double yaw;

        double rollG;

        double pitchG;


        private void parseContent(byte[] data)
        {
            int accX = (data[0] << 8) + data[1];
            int accY = (data[2] << 8) + data[3];
            int accZ = (data[4] << 8) + data[5];

            int temp = (data[6] << 8) + data[7];

            int gyroX = ((data[8] << 8) + data[9])/250;
            int gyroY = ((data[10] << 8) + data[11]) / 250;
            int gyroZ = ((data[12] << 8) + data[13]) / 250;

            double rollA_radians = Math.Atan2(accY, accZ);
            double pitchA_radians = -Math.Atan2(accX, accZ);

            double rollA = rollA_radians * (180 / Math.PI);
            double pitchA= pitchA_radians * (180 / Math.PI);

            int dt_nanoSecond = (data[14]<<24) + (data[15]<<16) + (data[16]<<8) + data[17];

            double dt_seconds = dt_nanoSecond / 1000_000d;

            yaw += gyroZ * dt_seconds;
            rollG += gyroX * dt_seconds;
            pitchG += gyroY * dt_seconds;

            double a = ParametersContext.a;

            double pitch = a * pitchG + (1-a) * pitchA;
            double roll = a * rollG + (1 - a) * rollA;

            ParametersContext.yaw = yaw;
            ParametersContext.pitch = pitch;
            ParametersContext.roll = roll;
          
        }

        

    }
}
